package org.usfirst.frc.team1322.robot.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.TalonSRX;

import org.usfirst.frc.team1322.robot.RobotMap;
import org.usfirst.frc.team1322.robot.commands.*;

/**
 *	TODO: Make sure this runs correctly
 */

public class GrabberSystem extends Subsystem {
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
	TalonSRX mGrabber;
	DigitalInput dGrabberInsideLimit, dGrabberOutsideLimit;
	private final double mPower = 1.0;
	
	public GrabberSystem(){
		mGrabber = new TalonSRX(RobotMap.pGrabber);
		dGrabberInsideLimit = new DigitalInput(RobotMap.pGrabberInsideLimit);
		dGrabberOutsideLimit = new DigitalInput(RobotMap.pGrabberOutsideLimit);
	}
	
	public void expandGrabber(){
		mGrabber.set(dGrabberOutsideLimit.get() ? 0 : mPower);
	}
	
	public void contractGrabber(){
		mGrabber.set(dGrabberInsideLimit.get() ? 0 : -mPower);
	}
	
	public void stopGrabber(){
		mGrabber.set(0);
	}
	
	public void setGrabberPower(double mPower){
		if ((dGrabberInsideLimit.get() && mPower < 0) || (dGrabberOutsideLimit.get() && mPower > 0)){
			this.stopGrabber();
		}
		else {
			mGrabber.set(mPower);
		}		
	}

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    	setDefaultCommand(new JoystickGrabber());
    }
}

